#include <gtk/gtk.h>
#include "main.h"
#include "netcom.h"
#include "pms5005.h"
#include "robot_window.h"

conn_t *conn;

void ping_robot(char *address, int port, conn_t **conn)
{
//	conn_t *conn;
	packet_t *packet;
	uint8_t stream[MAX_PACKET_LEN];

	g_print("giving the robot a ping :)\n");

	connection_new_robot(conn, address, port);	
	packet = ping_pms5005();
	packet_serialize(packet, stream);
	connection_send_stream(*conn, stream, packet_stream_length(stream));
	connection_get_data(*conn);
	
	g_print("address: %s; port: %d\n", address, port);
	packet_delete(packet);
}

void exec_action(int action_type, int amount, conn_t *conn)
{
//	conn_t *conn;
	packet_t *packet;
	uint8_t stream[MAX_PACKET_LEN];
	int16_t enc1 = conn->data->encoder_count[0],
		enc2 = conn->data->encoder_count[1];

//	connection_new_robot(&conn, address, port);	
	packet = dc_motor_polarity(0, 1);
	packet_serialize(packet, stream);
	packet_delete(packet);
	connection_send_stream(conn, stream, packet_stream_length(stream));
	connection_send_stream(conn, stream, packet_stream_length(stream));
	connection_send_stream(conn, stream, packet_stream_length(stream));
	connection_send_stream(conn, stream, packet_stream_length(stream));

	packet = dc_motor_polarity(1, 1);
	packet_serialize(packet, stream);
	packet_delete(packet);
	connection_send_stream(conn, stream, packet_stream_length(stream));
	connection_send_stream(conn, stream, packet_stream_length(stream));
	connection_send_stream(conn, stream, packet_stream_length(stream));
	connection_send_stream(conn, stream, packet_stream_length(stream));

	g_print("action type: %d; amount: %d\n", action_type, amount);

	switch ( action_type ) {
		case 0 :
			g_print("forward :%d", amount);
			packet = robot_move(conn->data, amount);
			
			break;
		case 1 :
			g_print("backward: %d", amount);
			packet = robot_move(conn->data, amount);
			sleep(5);
			break;
		case 2 :
			g_print("left");
			packet = robot_rotate(conn->data, amount);
			break;
		case 3 :
			g_print("right");
			packet = dc_motor_position(0, amount, 1, amount, 0);
			break;
		default : 
			break;
	}

	packet_serialize(packet, stream);
	connection_send_stream(conn, stream, packet_stream_length(stream));
	packet_delete(packet);
}

void send_halt()
{
	packet_t *packet = NULL;
	uint8_t stream[MAX_PACKET_LEN];

	g_print("Sending robot halt signal!\n");
	packet = dc_motor_state(DCMOTOR_OFF);

	packet_serialize(packet, stream);
	connection_send_stream(conn, stream, packet_stream_length(stream));
	packet_delete(packet);
}

int main(int argc, char *argv[])
{	
	GtkWidget *w = NULL;
	cb_widgets_t cb_widgets;

	g_thread_init(NULL);
	gdk_threads_init();
	gtk_init(&argc, &argv);

	w = robot_window_new(&cb_widgets);


	gtk_main();
	return 0;
}
